package novemberBot.controller;


import lejos.nxt.Motor;
import novemberBot.ArmRemote;
import novemberBot.FactoryFloorRobot;
import novemberBot.navigation.Navigator;
import novemberBot.navigation.Odometer;
import novemberBot.sensor.MotorisedUsS;
import novemberBot.sensor.UsDataResponder;
import novemberBot.utils.Trig;

/**proximity fetcher finds and picks up a pallet if it is in the usSensors LOS
 * @author Antoine Lennartz*/
public class ProximityFetcher extends UsDataResponder{
	
	/**default sweep speed*/
	private static final double SWEEP_SPEED = 50;
	
	/**link to robot*/
	private FactoryFloorRobot ffr;
	
	/**reference to low ultrasonic sensor*/
	private MotorisedUsS muss;

	
	/**reference to Arm Remote*/
	private ArmRemote arm;

	/**reference to robot navigator*/
	private Navigator theCaptain;
	
	/**reference to robot navigator*/
	private Odometer odo;
	
	/**Constructor*/
	public ProximityFetcher() {
		
		super("maxd", Sensor.LOW);
		ffr = FactoryFloorRobot.getInstance();
		muss = ffr.getUsPoller().getUsLow();
		arm = ffr.getArmRemote();
		theCaptain = ffr.getNavigator();
		odo = ffr.getOdometer();
		filter.setFilterAmount(5);
		filter.newMeasurement();
		filter.setMaxD(40);
		
	}
	

	
	
	/**Assumes the pallet is in the robot's FoV and that the robot is not moving.
	 * then drives up to a distance where it can safely 180
	 * and continue the pickup process */
	public void driveTo(){
		theCaptain = ffr.getNavigator();
		boolean located=false;
		int attempts=0;
		needsData=true;
		int dist=0, i=0, count=0, pdist=0;
		double ang=0, pang=0, firstAng=0, recentAng=0;
		double[] cpos = new double[3], retryAng = new double[]{-5, 5, -10, 10, -15, 15};
		muss.pauseSweep();
		muss.sweepTo(-60, SWEEP_SPEED, false);

		while(!located && attempts<6){

			muss.sweepTo(60, SWEEP_SPEED, true);
			filter.newMeasurement();

			while(muss.isSweeping()){

				recentAng=Trig.fixAngle2((getSensorPosition()[2]));
				i=filter.getData();
				if(i!=255){		
					dist+=i;
					count++;
					ang+=recentAng;
				}
			}
			if(count!=0) {
				located=true;
				dist/=count; ang/=count;
				if(recentAng>=56){//edge of vision
					ang+=Math.atan(5/dist);
				}
				pdist=dist; pang=ang;
				dist=0; ang=0;
			}
			muss.sweepTo(-60, SWEEP_SPEED, true);
			filter.newMeasurement();
			while(muss.isSweeping()){
				recentAng=Trig.fixAngle2((getSensorPosition()[2]));
				i=filter.getData();
				if(i!=255){
					dist+=i;
					count++;
					ang+=recentAng;
					
				}
			}
			if(count!=0) {
				located=true;
				dist/=count; ang/=count;
				if(recentAng<=-56){//edge of vision
					ang-=Math.atan(5/dist);
				}
				dist+=pdist; ang+=pang;
				dist/=2; ang/=2;
			}else if(!located){	
				odo.getPosition(cpos);
				theCaptain.setRotation(cpos[2]+retryAng[attempts], false);
				theCaptain.setForward(attempts, false);
				attempts++;
			}
		}

		if(!located){
			return;
		}

		firstAng=ang;

		theCaptain.setRotation(ang, false);

		theCaptain.setRotation(dist/3, false);

		//reset values for new sweep
		dist=0; ang=0; count=0;
		located=false;
		attempts=0;
		//sweep n measure x2
		while(!located && attempts<6){
			muss.sweepTo(60, SWEEP_SPEED, true);
			filter.newMeasurement();
			while(muss.isSweeping()){
				recentAng=Trig.fixAngle2((getSensorPosition()[2]));
				i=filter.getData();
				if(i!=255){
					dist+=i;
					count++;
					ang+=recentAng;
					
				}
			}

			if(count!=0){ 
				located=true;
				dist/=count; ang/=count;
				if(recentAng>=56){//edge of vision
					ang+=Math.atan(5/dist);
				}
				pdist=dist; pang=ang;
				dist=0; ang=0;
			}
			muss.sweepTo(-60, SWEEP_SPEED, true);
			filter.newMeasurement();
			while(muss.isSweeping()){
				recentAng=Trig.fixAngle2((getSensorPosition()[2]));
				i=filter.getData();
				if(i!=255){
					dist+=i;
					count++;
					recentAng=Trig.fixAngle2((getSensorPosition()[2]));
					ang+=recentAng;
				}
			}// end of sweep n measure x2
			if(count!=0){ 
				located=true;
				dist/=count; ang/=count;
				if(recentAng<=-56){//edge of vision
					ang-=Math.atan(5/dist);
				}
				dist+=pdist; ang+=pang;
				dist/=2; ang/=2;
			
				
			}else if(!located){
				odo.getPosition(cpos);
				theCaptain.setRotation(cpos[2]+retryAng[attempts], false);
				theCaptain.setForward(attempts, false);
				attempts++;	
			}
		}
		if(!located){
			return;
		}
		
		theCaptain.setRotation((3*ang+firstAng)/4, false);
		theCaptain.setForward(dist-26, false);
		odo.getPosition(cpos);
		//final 180
		theCaptain.setRotation(cpos[2]+180, false);
		//no longer needs sensor data
		needsData=false;
		muss.sweepTo(0, SWEEP_SPEED, false);
		muss.resumeSweep();

	}
	
	
	/**Picks up a pallet assumes that it is properly oriented 
	 * and in the right place*/
	public void pickUp(){
		theCaptain.setForward(-15, false);
		alignPallet();
		theCaptain.setForward(2, false);
		arm.liftClawTo(5, false);
		arm.openClawTo(70, false);
		arm.liftClawTo(0, false);
		arm.openClawTo(-25, false);
		ffr.setLoad(ffr.getLoad()+1);
		arm.liftClawTo(liftHeight(), false);
	}
	/**computes liftHeight based on Robot load and the assuption that there is
	 * a roof that it should try and squeeze the pallets against*/
	private int liftHeight(){
		if(ffr.getLoad()==1 || ffr.getLoad()==2) return 6;
		return 21-(ffr.getLoad()-3)*5;
	}
	
	/**aligns the pallet so it is straight*/
	private void alignPallet(){
		Motor.B.setSpeed(60);
		Motor.A.setSpeed(60);
		Motor.B.rotate(-60);
		Motor.A.rotate(-115);
		Motor.B.rotate(-52);
	}
	/**drops off the current stack of pallets in the claw*/
	public void dropOff(){
		arm.liftClawTo(0, false);
		arm.openClawTo(70, false);
		theCaptain.setForward(13, false);
		arm.openClawTo(0, false);
	}
	

}
